%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Original version Written by: Mohammad Rabaiah
% Downloaded from MATLAB Central
% Edited by Ben Weiss 9/11/2023
% Projection onto a plane
% The vector P is the orthogonal projection of b onto plane 
% which has two basis a1 and a2 
% The vector error "e" is perpendicular to the plane so that 
%               e = b - P
% Then you can check the orthogonality ==0
% let a1=[1;1;1] and  a2=[0;1;2]
%      b=[6;0;0]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function P = Projection3(a1,a2, B);
%clc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Enter the data 
%a1 = [0.8855; 0.4338; 0.1664];%[0.8414; 0.4530; 0.2947] % %a1=[1;1;1];
%a2 = [-0.4446; 0.895; 0.0326];%[-0.4958, 0.8641, 0.0871]% %a2=[0;1;2];
%B = [0; 0; -1]; %B=[6;0;0];
%nP is projection of P onto to normal to a1-a2 plane
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=[a1(1) a2(1);
    a1(2) a2(2);
    a1(3) a2(3)] ;
k=A'*A;
Ik=inv(k);
X=Ik*A'*B;
P=A*X/norm(A*X);
ERROR=B-P;
%disp(['**************************'])
%disp(['check orthogonality'])
ERROR'*P;
ERROR'*A(:,1);
ERROR'*A(:,2);

n = cross(a1, a2); %normal
'P cross n' ;
nP = dot(n,P);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
